#include "matlite.h"
#include "matlite.cpp"

//Constants
double PI=3.1415926536;
double PI2=1.5707963268;

//Utility functions
void normalise(double &val);
void minimum(int &ans,int val1,int val2);
void minRI(double &minVal,int &minInd,vector<double> array);
void state_remove_square(matlite &original,int state);
void state_remove_vector(matlite &original,int state);

class ekfblob{
        
        public:
        
        //EKF Functions
        void set(FILE *input,char *LOG_file);
        void update_existing(int j,int minI);
        void nearby_LM(vector<double> &dist_range,int j,double &minD,int &minI);
        void toxy(double range,double bearing,double &x,double &y);
        void checkfornewfeature();
        void predict(double d_d,double d_t);
        void update(playerc_blobfinder_blob_t *blob, int NBlob,double d_t,double lpdiff,matlite &ROBOTS);
        //void extractLM(int &numLM,double *lp);
        void extractLMfromBlob(playerc_blobfinder_blob_t *blob, int NBlob);
        //void blobtest(playerc_blobfinder_blob_t *blob, int NBlob);

        //Fixed
        matlite xest,Pest,A,Q,R,LMnoti; 
        float QnoiseR,QnoiseB,RnoiseR,RnoiseB;
        int laserproxies;
        float resolution;
        float max_range;
        float offset;
        int row;

        FILE *mapin;
        FILE *LOG1;
        FILE *lm;
        float initialP;
        int nLMnoti;
        
        //Must reset
        vector<double> LMbearing,LMrange,LMID;
        vector<double> tempx,tempy;
        vector<double> newx,newy,news;
        int unseencount;
        float separation;
        float min_dt,min_lpdiff;
        int seen;
        //float ox,oy,ot;
        float xt;
        float yt;
        float tt;       

};


//Read parameters from input.txt which will make tuning easier
//SQRT-square root the value inside input.txt to get the actual covariance value
//ALL ANGLES ARE IN RADIANS
//P: Pioneer P3-DX                      S: Stage
void ekfblob::set(FILE *input,char *LOG_file ){

        
        xest.resize(3,1);
        row=xest.nRow();
        Pest.eye(row,row);
        A.eye(row,row);
        Q.zeros(row,row);
        R.zeros(2,2);
        

//Log::ReportingLevel() = logINFO;
//Log().Get(logINFO) << "inizializzazione struttura "<< row;
	
        
        
    	  //  input=fopen("input.txt","r");
                LOG1=fopen(LOG_file,"w");

        
        
        fscanf(input,"%f",&xt);                 //initial x
        fscanf(input,"%f",&yt);                 //initial y
        fscanf(input,"%f",&tt);                 //initial orientation (rad)
        fscanf(input,"%f",&QnoiseR);            //q noise for range SQRT
        fscanf(input,"%f",&QnoiseB);            //q noise for bearing SQRT
        fscanf(input,"%f",&RnoiseR);            //r noise for range SQRT
        fscanf(input,"%f",&RnoiseB);            //r noise for bearing SQRT
        fscanf(input,"%d",&laserproxies);       //number of laser proxies (related to resolution)       P: 512  S:361
        fscanf(input,"%f",&resolution);         //angular resolution for laser                  P:0.3516        S:0.5
        fscanf(input,"%f",&max_range);          //ignore laser range readings > x metres        
        fscanf(input,"%f",&offset);             //laser offset from vehicle centre              P:0.18~0.2      S:0.00001
        fscanf(input,"%d",&unseencount);        //keep to zero
        fscanf(input,"%f",&separation);         //minimum distance between each landmark (0.3m or more for ICNN)
        fscanf(input,"%f",&initialP);           //initial covariance for newly initialised feature SQRT
        fscanf(input,"%f",&min_dt);             //only update when the robot isn't turning-fewer mistakes are likely to happen
        fscanf(input,"%f",&min_lpdiff);         //timestamp difference between laser and odometer should be less than 0.001s
        fscanf(input,"%d",&seen);
        


        
        xest.val[0][0]=xt;
        xest.val[1][0]=yt;
        xest.val[2][0]=tt;
        
        Q.val[0][0]=QnoiseR;    Q.val[1][1]=QnoiseR;    Q.val[2][2]=QnoiseB;
        R.val[0][0]=RnoiseR;    R.val[1][1]=RnoiseB;

        //ox=xt;
        //oy=yt;
        //ot=tt;

       // fclose(input);

//init landamrk noti
mapin=fopen("mapin.txt","r");
fprintf(LOG1,"loading landmark from file \n");
        int xL,yL,tL,idL,portL;
        fscanf(mapin,"%d",&nLMnoti);     // num of landmark in the map
        LMnoti.resize(5,nLMnoti);
        for (int k=0; k<nLMnoti; k++){
                
    fscanf(mapin,"%d",&xL);
    fscanf(mapin,"%d",&yL);
        fscanf(mapin,"%d",&tL);
        fscanf(mapin,"%d",&idL);
fscanf(mapin,"%d",&portL);
                
        LMnoti.val[0][k]=xL;
        LMnoti.val[1][k]=yL;
    LMnoti.val[2][k]=tL;
        LMnoti.val[3][k]=idL;
LMnoti.val[4][k]=portL;
        
                fprintf(LOG1,"%d :>[X: %f, Y: %f, t: %f, ID: %f,port: %f] \n",k+1,LMnoti.val[0][k],LMnoti.val[1][k],LMnoti.val[2][k],LMnoti.val[3][k],LMnoti.val[4][k]);        
        }
        fclose(mapin);  
};

//EKF prediction
void ekfblob::predict(double d_d,double d_t){

        xest.val[0][0]+=d_d*cos(xest.val[2][0]);
        xest.val[1][0]+=d_d*sin(xest.val[2][0]);
        xest.val[2][0]+=d_t;
        normalise(xest.val[2][0]);
        
        A.val[0][0]=1.0;                A.val[0][1]=0;                  A.val[0][2]=-d_d*sin(xest.val[2][0]);
        A.val[1][0]=0;                  A.val[1][1]=1.0;                A.val[1][2]=d_d*cos(xest.val[2][0]);
        A.val[2][0]=0;                  A.val[2][1]=0;                  A.val[2][2]=1.0;
                
        Q.val[0][0]=QnoiseR;    
        Q.val[1][1]=QnoiseR;    
        Q.val[2][2]=QnoiseB;    
        
        matlite Atemp;
        matMult(Atemp,A,Pest);
        matMultAdd(Pest,Atemp,A,Q,true);
        
}

//Convert range and bearing readings to x,y coordinates
void ekfblob::toxy(double range,double bearing,double &x,double &y){
        normalise(xest.val[2][0]);
        x=xest.val[0][0]+range*cos(xest.val[2][0]+bearing);
        y=xest.val[1][0]+range*sin(xest.val[2][0]+bearing);
}


//This is where the actual EKF SLAM update equations take place
void ekfblob::update_existing(int j,int minI){
        //int closest=3+(2*(int)minI);
        int closest= minI;

    double range,angle;
        //EKF Update for each landmark
        double drx=LMnoti.val[0][closest]-xest.val[0][0];
        double dry=LMnoti.val[1][closest]-xest.val[1][0];
        range=sqrt(drx*drx+dry*dry);
        angle=atan2(dry,drx)-xest.val[2][0];//+PI2;
        normalise(angle);

        matlite Jh;
        Jh.zeros(2,xest.nRow());
        Jh.val[0][0]=-drx/range;                    Jh.val[0][1]=-dry/range;                    Jh.val[0][2]=0.0;
        Jh.val[1][0]=dry/(range*range);         Jh.val[1][1]=-drx/(range*range);                Jh.val[1][2]=-1.0;
        //      Jh.val[0][closest]=drx/range;           Jh.val[0][closest+1]=dry/range;
        //Jh.val[1][closest]=-dry/(range*range);        Jh.val[1][closest+1]=drx/(range*range);

        //Innovation Matrices
        matlite innov(2,1);
        innov.val[0][0]=(LMrange[j]-range);
        innov.val[1][0]=LMbearing[j]-angle;
        normalise(innov.val[1][0]);
        innov.val[1][0]=innov.val[1][0];

        //EKF Update Phase
        //K matrix=P.Jh'.S^-1
        matlite K,temp1,temp2,temp3,Kinnov,S,Sinv;
        matMult(temp1,Jh,Pest);
        matMultAdd(S,temp1,Jh,R,true);

        //State Update
        //Xest=Xest+K*innov
        matinv(Sinv,S);

        //S.choleskyInv();
        matMult(temp2,Pest,Jh,true);
        matMult(K,temp2,Sinv);
        matMult(Kinnov,K,innov);
        matAdd(xest,xest,Kinnov);
        normalise(xest.val[2][0]);

        //Pest Update
        //Pest=(I-K*J)*Pest
        matlite PIdentity;
        matMult(temp3,K,Jh);
        PIdentity.eye(temp3.nRow(),temp3.nCol());
        matMinus(temp3,PIdentity,temp3);
        matlite Pold(Pest);
        matMult(Pest,temp3,Pold);

        
}

void ekfblob::update(playerc_blobfinder_blob_t *blob, int NBlob,double d_t,double lpdiff,matlite &ROBOTS){
        lm=fopen("output_data/LM.txt","a");
        LMrange.clear();LMbearing.clear();LMID.clear();
        tempx.clear();tempy.clear();
        unseencount=0;

// estrae dal  laser data la posizione degli oggetti che trova in quella data pose
        if(d_t<abs(min_dt) && lpdiff<min_lpdiff){
                //extractLM(numLM,laserdata);
                extractLMfromBlob (blob,NBlob);
 printf("QUI - debug i\n");
                                fprintf(LOG1,"------>numer of landmark reacted %i \n", NBlob);



// in questa fase  l'algo deve verificare se il lm trovato appartiene alla lista dei 
//              lm noti e se esso sta nella posizione in cui deve essere.

                
        
                for(int j=0;j<NBlob;j++){
                        //Check if found landmarks
                        double xlm,ylm;
                        toxy(LMrange[j],LMbearing[j],xlm,ylm);
                                                        fprintf(LOG1,"--> landmark %i reacted [x,y]= [%f,%f], ID: %f\n", j+1,xlm,ylm,LMID[j]);
        //    Matching id color 
                        // ritorna l'indice del landmark noto con id = ID
                        int ID= LMID[j];
                        int ind_match=0;
                        for(int k=0;k<LMnoti.nCol();k=k+1){
                        if (ID==LMnoti.val[3][k]){
                        ind_match=k;
                        fprintf(LOG1,"--> landmark noto with the same ID: %f is in the pos: [%f,%f] \n", LMnoti.val[3][ind_match],LMnoti.val[0][ind_match],LMnoti.val[1][ind_match]);
                        }
                }


                        //double minD;
                        //int minI;
                        //vector<double> dist_range;
                        //dist_range=0;
                        //nearby_LM(dist_range,j,minD,minI);
                        //classical one 
                        //if(minI==ind_match){
                        //if(minD<separation ){
                                //If the distance between a observation and a known feature is below the separation threshold, the innovation between the two is used to update everything.
                                //std::cout << "landmark reacted " << LMnoti.val[3][(int)minI];
                        //      fprintf(LOG1,"------>landmark noto found [x,y]: [%f,%f] ID: %f", LMnoti.val[0][(int)minI],LMnoti.val[1][(int)minI],LMnoti.val[3][(int)minI]);
                        //      fprintf(LOG1,"alla distanza di %f \n ",minD);
                        //      update_existing(j,minI);
                          //      fprintf(lm,"%f %f ",LMnoti.val[0][(int)minI],LMnoti.val[1][(int)minI]);
                                //fprintf(lm,"%f %f ",xlm,ylm);


                        //} else {
                        //      fprintf(LOG1,"------>replace lmnoti def [x,y]: [%f,%f] with [%f,%f]\n", LMnoti.val[0][(int)minI],LMnoti.val[1][(int)minI],xlm,ylm);



// GETTING THE REAL POSITION OF THE LM FROM THE TABLE ROBOTS
int port=LMnoti.val[4][ind_match];
int ind_match_R=-1;
        for(int k=0;k<ROBOTS.nCol();k=k+1){
                        if (port==ROBOTS.val[3][k]){
                        ind_match_R=k;

}

}

						if(ind_match_R>-1){
                                                                LMnoti.val[0][ind_match]= ROBOTS.val[0][ind_match_R];
                                                                LMnoti.val[1][ind_match]= ROBOTS.val[1][ind_match_R];
                                                                update_existing(j,ind_match);
                                                                fprintf(lm,"%f %f ",LMnoti.val[0][ind_match],LMnoti.val[1][ind_match]);


                        
                                                normalise(xest.val[2][0]);
						}
        }
}
        fclose(lm);
}
//Extracts a array of range and bearing readings
//LMrange and LMbearing
//ONYL FOR CIRCULAR FEATURES
void ekfblob::extractLMfromBlob(playerc_blobfinder_blob_t *blob, int NBlob){

                        for (int i=0; i<NBlob;i++){
                                playerc_blobfinder_blob_t b = blob[i];
                                float range=b.range;
                                int index=357-b.x;
                                int y= b.y;
                                int col= b.color;
                                //printf("range for the blob: %f \n",range);
                                //printf("coordinate in pixel : %d %d \n",index,y);
                                double bearing= index*resolution*PI/180.0 -PI2;
                   			LMrange.push_back(range);
                                        LMbearing.push_back(bearing);
                    			LMID.push_back(col);
                                
}
}


//Finds the minimum distance between an observation and a known feature which is used during data association. If the minimum distance is below the threshold (0.3m in the default code), it is assumed by DA as a match and then used during the update step.
void ekfblob::nearby_LM(vector<double> &dist_range,int j,double &minD,int &minI){

        //Find coordinates of each individual landmark
        double xlm,ylm,dx2,dy2;
        toxy(LMrange[j],LMbearing[j],xlm,ylm);
                        
        for(int k=0;k<LMnoti.nCol();k=k+1){
                dx2=xlm-LMnoti.val[0][k];
                dy2=ylm-LMnoti.val[1][k];
                dist_range.push_back( sqrt(  dx2*dx2+dy2*dy2));
        }

        minRI(minD,minI,dist_range);


}


//Utility Functions

//Return the smallest value between 2 numbers
void minimum(int &ans,int val1,int val2){
        if(val1>val2){          ans=val2;       }
        else{                   ans=val1;       }
}

//Returns the smallest value inside an array
void minRI(double &minVal,int &minInd,vector<double> array){
        minVal=array[0];
        minInd=0;
        for(int i=1;i<(int)array.size();i++){
                if(array[i]<minVal){
                        minVal=array[i];
                        minInd=i;
                }
        }
}

//Keep angles within +2PI~-2PI (used for vehicle orientation)
void normalise(double &val){
        if(val>=PI){
                val-=2*PI;
        }
        else if(val<=-PI){
                val+=2*PI;
        }
}


//You can use this to remove rows and columns in a square matrix (for feature removal)
void state_remove_square(matlite &original,int state){
        matlite tempo;
        tempo.zeros(original.nRow()-1,original.nCol()-1);
        int io,jo;
        for(int i=0;i<tempo.nRow();i++){
                jo=0;
                io=i;
                if(i>=state){
                        io++;
                }
                for(int j=0;j<tempo.nCol();j++){
                        if(j==state){
                                jo++;
                        }
                                
                        tempo.val[i][j]=original.val[io][jo];
                        jo++;
                }
        }
        original.resize(tempo.nRow(),tempo.nCol());
        for(int i=0;i<tempo.nRow();i++){
                for(int j=0;j<tempo.nCol();j++){
                        original.val[i][j]=tempo.val[i][j];
                }
        }
        original.show();

}

//You can use this to remove elements inside a state vector (for feature removal)
void state_remove_vector(matlite &original,int state){
        matlite tempv;
        tempv.zeros(original.nRow()-1,1);
        int io=0;
        for(int i=0;i<tempv.nRow();i++){
                io=i;
                if(i>=state){
                        io++;
                }
                tempv.val[i][0]=original.val[io][0];
        }
        original.resize(tempv.nRow(),1);
        for(int i=0;i<tempv.nRow();i++){
                original.val[i][0]=tempv.val[i][0];
        }
        original.show();
}

